#include "common_inc.h"
#include "configurations.h"
#include <string.h>
#include <stdlib.h>
#include "bsp_uart.h"
#include "bsp_can.h"
#include "can.h"
#include "usart.h"

extern Motor motor;
extern EncoderCalibrator encoderCalibrator;


#define debugPrintf(str, ...) 

float rxfdata[2] = {0};
bool debugOutputFlag = 0;

void OnUartCmd(void *arg, uint8_t* data, uint16_t len)
{
    uint8_t id, cmd, checkSum;
    uint8_t *RxData = NULL;
    float tmpF;
    while (arg == &huart1)
    {
        if (data[0] != 0x3C || len < 12)
        {
            break;
        }
        checkSum = check_sum(&data[1], 10); // 校验和
        if (checkSum != data[11])
        {
            UARTx_Printf((UART_HandleTypeDef*)arg, "checksum error:exp(%X),act(%X)\r\n", checkSum, data[11]);
            break;
        }
        id  = data[1] & 0x0F; // 4Bits ID
        cmd = data[2] & 0x7F; // 7Bits Msg Cmd
        memcpy(rxfdata, &data[3], 8);
        RxData = (uint8_t*)rxfdata;   // index[0..7]为data
        if (id == 0 || id != boardConfig.canNodeId) //转发ID为0或者非本节点ID的消息
        {
            if (cmd != 0x11) // 不广播ID设置命令
            {
                // 将指令转发到CAN总线
                UARTx_Printf((UART_HandleTypeDef*)arg, "forward cmd to CAN\r\n");
                UARTx_Printn((UART_HandleTypeDef *)arg, data, len);
                CAN_Send_Data(id, cmd, RxData);
            }
            if (id != 0) // 不是本设备的命令直接退出
            {
                return;
            }
        }
        interface_base(arg, cmd, RxData);
		return;
    }

#if 1 // string command
    if (data[0] != '>') // head char is '>'
    {
        return;
    }
    RxData = &data[1];
    uint8_t ok_flag = 0;
    for (size_t i = 0; i < len-1; i++)
    {
        if (RxData[i] == '\n')
        {
            ok_flag = 1;
            break;
        }
    }
    if (!ok_flag) // 没有帧尾
    {
        return;
    }

    switch (*RxData)
    {
        case 'a': // a10 (加速度r/s^2)
            tmpF = atof((char*)&RxData[1]) * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
            motor.config.motionParams.ratedVelocityAcc = (int32_t) tmpF;
            motor.motionPlanner.velocityTracker.SetVelocityAcc((int32_t) tmpF);
            motor.motionPlanner.positionTracker.SetVelocityAcc((int32_t) tmpF);
            boardConfig.velocityAcc = motor.config.motionParams.ratedVelocityAcc;
            ok_flag = 1;
            break;
        case 'l': // 速度限制(r/s)
            tmpF = atof((char*)&RxData[1]);
            motor.config.motionParams.ratedVelocity =
                (int32_t) (tmpF * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);
            boardConfig.velocityLimit = motor.config.motionParams.ratedVelocity;
            ok_flag = 1;
            break;
        case 'c': // c1.3 (A)
            tmpF = atof((char*)&RxData[1]);
            if (motor.controller->modeRunning != Motor::MODE_COMMAND_CURRENT)
                motor.controller->SetCtrlMode(Motor::MODE_COMMAND_CURRENT);
            motor.controller->SetCurrentSetPoint((int32_t) (tmpF * 1000));
            ok_flag = 1;
            break;
        case 'v': // v2.0 (r/s)
            tmpF = atof((char*)&RxData[1]);
            if (motor.controller->modeRunning != Motor::MODE_COMMAND_VELOCITY)
            {
                motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
                motor.controller->SetCtrlMode(Motor::MODE_COMMAND_VELOCITY);
            }
            motor.controller->SetVelocitySetPoint(
                (int32_t) (tmpF * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));
            ok_flag = 1;
            break;
        case 'p': // p2.5 (circle)
            tmpF = atof((char*)&RxData[1]);
            if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
                motor.controller->requestMode = Motor::MODE_COMMAND_POSITION;

            motor.controller->SetPositionSetPoint(
                (int32_t) (tmpF * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));
            ok_flag = 1;
            break;
        case 'i':// set id (1~15) eg: i5
            boardConfig.canNodeId = atoi((char*)&RxData[1]) & 0x0F;
            boardConfig.configStatus = CONFIG_COMMIT;
            UARTx_Printf((UART_HandleTypeDef *)arg, "Node ID: %d\r\n", boardConfig.canNodeId);
            ok_flag = 1;
            break;
        case 'j': // Do Calibration
            encoderCalibrator.isTriggered = true;
            ok_flag = 1;
            break;
        case 'e': // enable motor e1/e0
            motor.controller->requestMode = (RxData[1] == '1') ?
                                            Motor::MODE_COMMAND_VELOCITY : Motor::MODE_STOP;
            ok_flag = 1;
            break;
        case 'z': // Apply Home-Position
            motor.controller->ApplyPosAsHomeOffset();
            boardConfig.encoderHomeOffset = motor.config.motionParams.encoderHomeOffset %
                                            motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
            ok_flag = 1;
            break;
        case 'k':
            ok_flag = 1;
            switch (RxData[1])
            {
            case 'p':
                motor.config.ctrlParams.dce.kp = atoi((char*)&RxData[2]);
                boardConfig.dce_kp = motor.config.ctrlParams.dce.kp;
                break;
            case 'i':
                motor.config.ctrlParams.dce.ki = atoi((char*)&RxData[2]);
                boardConfig.dce_ki = motor.config.ctrlParams.dce.ki;
                break;
            case 'd':
                motor.config.ctrlParams.dce.kd = atoi((char*)&RxData[2]);
                boardConfig.dce_kd = motor.config.ctrlParams.dce.kd;
                break;
            case 'v':
                motor.config.ctrlParams.dce.kv = atoi((char*)&RxData[2]);
                boardConfig.dce_kv = motor.config.ctrlParams.dce.kv;
                break;
            case 'e': // 串口调试输出
                debugOutputFlag = RxData[2] == '1';
                break;
            default:
                ok_flag = 0;
                break;
            }
            break;
        case 's': // save
            boardConfig.configStatus = CONFIG_COMMIT;
            break;
        default:
            UARTx_Print((UART_HandleTypeDef *)arg, "Only support [a] [l] [c] [v] [p] [i] [j] [e] [z] [k] commands!\r\n");
            break;
    }
    if (ok_flag)
    {
        UARTx_Printn((UART_HandleTypeDef *)arg, data, len);
        UARTx_Print((UART_HandleTypeDef *)arg, "OK\r\n");
    }
#endif

}



